#include "state_manager.h"

#define GET_SYS_TIME() board_get_systime()

static struct state_param {
	int16_t minimal_throttle_input;
	int16_t sidemost_thresh;
	uint64_t try_arming_disarming_duaration;
} option = {
	.minimal_throttle_input = 50,
	.sidemost_thresh = 50,
	.try_arming_disarming_duaration = 1e6
};

void land_detection(struct vehicle_instance *vehicle);
void update_arm_status(struct vehicle_instance *vehicle);
void update_flight_mode(struct vehicle_instance *vehicle);

void state_init(struct vehicle_status *vehicle_stat)
{
	vehicle_stat->flight_mode = MODE_STABLIZED;
	vehicle_stat->arm_status = DISARMED;

	vehicle_stat->mag_calibrated = 0;
	vehicle_stat->landed = 1;
	vehicle_stat->last_update_time = GET_SYS_TIME();
	vehicle_stat->try_arming_disarming_start_time =
		vehicle_stat->last_update_time;
	vehicle_stat->mode_switch_position = 0;
	vehicle_stat->mode_switch_counter = 0;
}

void task_state_update(void)
{
#if 0
	float mode_switch_change = 0.0f;
	g_vehicle_status.last_update_time = GET_SYS_TIME();
	land_detection(&g_vehicle_status);
	/* vehicle status transition is driven by rc input */
	if (!g_rc.valid) {
		return;
	}

	update_arm_status(&g_vehicle_status, &g_rc);

	/*
	 * we only have the 5th channel, a 2 position switch to
	 * choose flight mode
	 */
	mode_switch_change = g_vehicle_status.mode_switch_position - g_rc.channels[4];
	if (mode_switch_change <= 1.0f && mode_switch_change >= -1.0f) {
		g_vehicle_status.mode_switch_counter = 0;
		return;
	}
	g_vehicle_status.mode_switch_counter++;
	if (g_vehicle_status.mode_switch_counter < 10)
		return;
	g_vehicle_status.mode_switch_counter = 0;
	g_vehicle_status.mode_switch_position = g_rc.channels[4];
	update_flight_mode(&g_vehicle_status, &g_rc);
#endif
}

void land_detection(struct vehicle_instance *vehicle)
{
}

void update_arm_status(struct vehicle_instance *vehicle)
{
#if 0
	switch (vehicle_stat->arm_status) {
		/* on DISARMED stage, we can do sensor calibration
		* by switch rc channel 4 on (pwm width larger than 0.5).
		* or we can arm the motors by pull yaw stick right-most.
		* 在上锁状态下，横滚打到最左进入标定模式；
		* 偏航打到最左并保持一秒钟解除锁定
		*/
	case DISARMED:
		/* determine if sensor calibration is needed */
		if (rcin->channels[0] > 0.8f && vehicle_stat->mag_calibrated == 0) {
			vehicle_stat->arm_status = IN_IMU_CALIBRATION;
			break;
		}
		if (rcin->channels[0] < 0.5) {
			vehicle_stat->mag_calibrated = 0;
		}
		if (rcin->channels[0] < -0.8f) {
			vehicle_stat->arm_status = IN_MAG_CALIBRATION;
			vehicle_stat->mag_calibrated = 1;
			break;
		}

		/* try arming */
		if (rcin->channels[2] > 1.0f - option.sidemost_thresh) {
			if (vehicle_stat->last_update_time - vehicle_stat->try_arming_disarming_start_time > option.try_arming_disarming_duaration) {
				vehicle_stat->arm_status = ARMED_IDLE;
				vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
			}
			break;
		}
		vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
		break;

		/* on ARMED_IDLE stage, we can enable the att controller by rising
		* throttle input above certain threshold.
		* Here 0.05 is used for jitter suppression.
		* or we can pull the yaw stick left-most to disarm the motors
		* 在解除锁定状态下，油门杆收至最低时，电机处于怠速状态。偏航杆打到最右并
		* 保持1秒钟，锁定电机
		*/
	case ARMED_IDLE:
		if (rcin->channels[3] > option.minimal_throttle_input + 0.05) {
			vehicle_stat->arm_status = ARMED_ACTIVATED;
			break;
		}
		if (rcin->channels[2] < -1.0f + option.sidemost_thresh) {
			if (vehicle_stat->last_update_time - vehicle_stat->try_arming_disarming_start_time > option.try_arming_disarming_duaration) {
				vehicle_stat->arm_status = DISARMED;
				vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
			}
			break;
		}
		vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
		break;
		/* 在
		*/
	case ARMED_ACTIVATED:
		if (rcin->channels[3] < option.minimal_throttle_input) {
			vehicle_stat->arm_status = ARMED_IDLE;
			vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
		}
		break;
		/* automatically exit IN_CALIBRATION mode when calibration complete
		*/
	case IN_IMU_CALIBRATION:
#define CALIBRATION_COMPLETE() 1
			if (!CALIBRATION_COMPLETE()) {
				break;
			}
			vehicle_stat->arm_status = DISARMED;
			vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
			break;
	case IN_MAG_CALIBRATION:
		if (rcin->channels[0] > 0.8f) {
			vehicle_stat->arm_status = DISARMED;
			vehicle_stat->try_arming_disarming_start_time = vehicle_stat->last_update_time;
		}
		break;
		default:
			break;
	}
#endif
}

void update_flight_mode(struct vehicle_instance *vehicle)
{
#if 0
	if (vehicle_stat->mode_switch_position > 0.0) {
		/* try switch to pos control mode */
#define POS_CTRL_INIT() position_control_init()	//TODO
		switch (POS_CTRL_INIT()) {
			case POS_CONTROL_SUFFICIENT:
				vehicle_stat->flight_mode = MODE_LOITER;
				break;
			case ALT_CONTROL_SUFFICIENT:
				vehicle_stat->flight_mode = MODE_ALT_HOLD;
				break;
			default:
				break;
		}
	} else {
		/*switch to stablized mode */
		vehicle_stat->flight_mode = MODE_STABLIZED;
	}
#endif
}
